#include "MeOrion.h"
#include <SoftwareSerial.h>

#define ANGLE_MAX 180
#define ANGLE_MIN 0
#define ANGLE1_MIN 100
#define ANGLE1_MAX 160
#define ANGLE2_MIN 30
#define ANGLE2_MAX 90
#define LIMIT_ANGLE1_MAX(angle) (angle=(angle>ANGLE1_MAX?ANGLE1_MAX:angle))
#define LIMIT_ANGLE1_MIN(angle) (angle=(angle<ANGLE1_MIN?ANGLE1_MIN:angle))
#define LIMIT_ANGLE2_MAX(angle) (angle=(angle>ANGLE2_MAX?ANGLE2_MAX:angle))
#define LIMIT_ANGLE2_MIN(angle) (angle=(angle<ANGLE2_MIN?ANGLE2_MIN:angle))
#define LIMIT_ANGLE(angle) (angle=(angle>ANGLE_MAX?ANGLE_MAX:(angle<ANGLE_MIN?ANGLE_MIN:angle)))
#define LIMIT_MAX_ANGLE(angle) (angle=(angle>ANGLE_MAX?ANGLE_MAX:angle))
#define LIMIT_MIN_ANGLE(angle) (angle=(angle<ANGLE_MIN?ANGLE_MIN:angle))
#define LIMIT_ANGLE(angle) (angle=(angle>ANGLE_MAX?ANGLE_MAX:(angle<ANGLE_MIN?ANGLE_MIN:angle)))

/***********************************/
/*          TFT LCD屏接口          */
/* 分辨率320x240，横轴为X，纵轴为Y */
/***********************************/
MeSerial lcdSerial(PORT_6); 

/***********************************/
/*          舵机控制接口           */
/***********************************/
MePort port(PORT_4);

int port1 = port.pin1();
int port2 = port.pin2();

Servo servo1;
Servo servo2;

int angle1 = 0;
int angle2 = 0;

/***********************************/
/*          红外遥控接口           */
/***********************************/
MeInfraredReceiver IR(PORT_6);

void OpenClaw(void)
{
  angle1 = ANGLE1_MAX;
  angle2 = ANGLE2_MIN;
}

void CloseClaw(void)
{
  angle1 = ANGLE1_MIN;
  angle2 = ANGLE2_MAX;
}

void Action(void)
{
  servo1.write(angle1);
  servo2.write(angle2);
}
void IRCodeProcess(uint8_t IRCode)
{
  switch(IRCode)
    {
       case IR_BUTTON_A: 
       {
        OpenClaw();
       }break;
       case IR_BUTTON_B: 
       {
        CloseClaw();
       }break;
       case IR_BUTTON_C: Serial.println("Press C."); break;
       case IR_BUTTON_D: Serial.println("Press D."); break;
       case IR_BUTTON_E: Serial.println("Press E."); break;
       case IR_BUTTON_F: Serial.println("Press F."); break;
       case IR_BUTTON_SETTING: Serial.println("Press Setting."); break;
       case IR_BUTTON_UP: 
       {
        angle1++;
        LIMIT_ANGLE1_MAX(angle1);
       }break;
       case IR_BUTTON_DOWN:
       {
        angle1--;
        LIMIT_ANGLE1_MIN(angle1);
       }break;
       case IR_BUTTON_LEFT:
       {
        angle2--;
        LIMIT_ANGLE2_MIN(angle2);
       }break;
       case IR_BUTTON_RIGHT:
        {
        angle2++;
        LIMIT_ANGLE2_MAX(angle2);
       }break;
       case IR_BUTTON_0:
       {
        angle1 = angle2 = 0;
       }break;
       case IR_BUTTON_1:
       {
        angle1 = angle2 = 18;
       }break;
       case IR_BUTTON_2:
       {
        angle1 = angle2 = 36;
       }break;
       case IR_BUTTON_3: 
       {
        angle1 = angle2 = 54;
       }break;
       case IR_BUTTON_4:
       {
        angle1 = angle2 = 72;
       }break;
       case IR_BUTTON_5:
       {
        angle1 = angle2 = 90;
       }break;
       case IR_BUTTON_6:
       {
        angle1 = angle2 = 108;
       }break;
       case IR_BUTTON_7: 
       {
        angle1 = angle2 = 126;
       }break;
       case IR_BUTTON_8: 
       {
        angle1 = angle2 = 144;
       }break;
       case IR_BUTTON_9: 
       {
        angle1 = angle2 = 162;
       }break;
       default: break;
    }
}

void setup() 
{
  lcdSerial.begin(9600);   //配置TFT LCD串口波特率
  Serial.begin(115200);    //配置PC串口波特率
  IR.begin();
  servo1.attach(port1);
  servo2.attach(port2);
  OpenClaw();
  Action();
  Serial.println("setup done!");
}

void loop() 
{
   if(IR.available())
  {
    uint8_t IRCode = IR.read();
    IRCodeProcess(IRCode);
    Serial.print("angle1=");
    Serial.print(angle1);
    Serial.print(",angle2=");
    Serial.println(angle2);
    Action();
  }
}
